#include "stdlib.h"
#include "string.h"

#include "gen_blkqueue.h"

void *queue_memcpy(void *dest, const void *src, size_t size)
{
    uint8_t *ptr_dest = dest;
    const uint8_t *ptr_src = src;
    while (size--)
        *ptr_dest++ = *ptr_src++;
    return dest;
}
#define _memcpy(dest, src, size) queue_memcpy(dest, src, size)

void gen_blk_queue_init(struct gen_blk_queue *queue, void *mempool, int blk_size, int mempool_size)
{
    queue->mempool = mempool;

    queue->block_size = blk_size;
    queue->block_num = mempool_size / queue->block_size;

    queue->read_index = queue->read_mirror = 0;
    queue->write_index = queue->write_mirror = 0;
}

int gen_blk_queue_push(struct gen_blk_queue *queue, const uint8_t *ptr, int num)
{
    if (!queue || !ptr)
        return 0;

    int free_num = gen_blk_queue_space_len(queue);  // 获取空闲块数量

    if (!free_num)
        return 0;

    if (num > free_num)
        num = free_num;

    if (queue->block_num - queue->write_index > num)    // 当前的push操作不会发生翻转
    {
        _memcpy(&queue->mempool[queue->write_index * queue->block_size], ptr, num * queue->block_size);
        queue->write_index += num;
    }
    else                                                // 如果发生翻转 则需要两次memcpy
    {
        _memcpy(&queue->mempool[queue->write_index * queue->block_size],
                ptr,
                (queue->block_num - queue->write_index) * queue->block_size);
        _memcpy(queue->mempool,
                &ptr[(queue->block_num - queue->write_index) * queue->block_size],
                (num - (queue->block_num - queue->write_index)) * queue->block_size);

        queue->write_mirror = ~queue->write_mirror;
        queue->write_index = num - (queue->block_num - queue->write_index);
    }
    return num;
}
int gen_blk_queue_push_block(struct gen_blk_queue *queue, const uint8_t *ptr)
{
    return gen_blk_queue_push(queue, ptr, 1);
}

int gen_blk_queue_push_force(struct gen_blk_queue *queue, const uint8_t *ptr, int num)
{
    if (!queue || !ptr)
        return 0;

    int free_num = gen_blk_queue_space_len(queue);  // 获取空闲块数量

    if (num > queue->block_num)
    {
        ptr = &ptr[(num - queue->block_num) * queue->block_size];
        num = queue->block_num;
    }

    if (queue->block_num - queue->write_index > num)    // 当前的写操作不会发生翻转
    {
        _memcpy(&queue->mempool[queue->write_index * queue->block_size], ptr, num * queue->block_size);
        queue->write_index += num;

        if (num > free_num)                             // 如果写操作覆盖了旧数据，则需要更新读索引到新的数据起始位置
            queue->read_index = queue->write_index;
    }
    else                                                // 如果发生翻转 则需要两次memcpy
    {
        _memcpy(&queue->mempool[queue->write_index * queue->block_size],
                ptr,
                (queue->block_num - queue->write_index) * queue->block_size);
        _memcpy(queue->mempool,
                &ptr[(queue->block_num - queue->write_index) * queue->block_size],
                (num - (queue->block_num - queue->write_index)) * queue->block_size);

        queue->write_mirror = ~queue->write_mirror;
        queue->write_index = num - (queue->block_num - queue->write_index);

        if (num > free_num)                             // 如果写操作覆盖了旧数据，则需要更新读索引到新的数据起始位置并翻转
        {
#if 0
            queue->read_mirror = ~queue->read_mirror;
#else
            queue->read_mirror = ~queue->write_mirror;
#endif
            queue->read_index = queue->write_index;
        }
    }
    return num;
}
int gen_blk_queue_push_block_force(struct gen_blk_queue *queue, const uint8_t *ptr)
{
    return gen_blk_queue_push_force(queue, ptr, 1);
}

int gen_blk_queue_pop(struct gen_blk_queue *queue, uint8_t *ptr, int num)
{
    if (!queue)
        return 0;

    int used_size = gen_blk_queue_block_num(queue);

    if (!used_size)
        return 0;

    if (num > used_size)
        num = used_size;

    if (queue->block_num - queue->read_index > num)
    {
        if (ptr)
            _memcpy(ptr, &queue->mempool[queue->read_index * queue->block_size], num * queue->block_size);
        queue->read_index += num;
    }
    else
    {
        if (ptr)
        {
            _memcpy(ptr,
                    &queue->mempool[queue->read_index * queue->block_size],
                    (queue->block_num - queue->read_index) * queue->block_size);
            _memcpy(&ptr[(queue->block_num - queue->read_index) * queue->block_size],
                    queue->mempool,
                    (num - (queue->block_num - queue->read_index)) * queue->block_size);
        }

        queue->read_mirror = ~queue->read_mirror;
        queue->read_index = num - (queue->block_num - queue->read_index);
    }
    return num;
}
int gen_blk_queue_pop_block(struct gen_blk_queue *queue, uint8_t *ptr)
{
    return gen_blk_queue_pop(queue, ptr, 1);
}
int gen_blk_queue_read(struct gen_blk_queue *queue, uint8_t *ptr, int num)
{
    if (!queue || !ptr)
        return 0;

    int used_size = gen_blk_queue_block_num(queue);

    if (!used_size)
        return 0;

    if (num > used_size)
        num = used_size;

    if (queue->block_num - queue->read_index > num)
    {
        _memcpy(ptr, &queue->mempool[queue->read_index * queue->block_size], num * queue->block_size);
    }
    else
    {
        _memcpy(ptr,
                &queue->mempool[queue->read_index * queue->block_size],
                (queue->block_num - queue->read_index) * queue->block_size);
        _memcpy(&ptr[(queue->block_num - queue->read_index) * queue->block_size],
                queue->mempool,
                (num - (queue->block_num - queue->read_index)) * queue->block_size);
    }
    return num;
}

int gen_blk_queue_read_offset(struct gen_blk_queue *queue, uint8_t *ptr, int offset, int num)
{
    if (!queue || !ptr || offset < 0 || num <= 0)
        return 0;

    int used_size = gen_blk_queue_block_num(queue);

    if ((offset + num) > used_size)     // 计算可读数量
    {
        num -= (offset + num) - used_size;
    }
    if (num <= 0)
    {
        return 0;
    }
    int read_index = queue->read_index + offset;
    if (read_index > queue->block_num)  // offset导致了翻转
    {
        read_index -= queue->block_num;
    }

    if (queue->block_num - read_index > num)
    {
        _memcpy(ptr, &queue->mempool[read_index * queue->block_size], num * queue->block_size);
    }
    else
    {
        _memcpy(ptr,
                &queue->mempool[read_index * queue->block_size],
                (queue->block_num - read_index) * queue->block_size);
        _memcpy(&ptr[(queue->block_num - read_index) * queue->block_size],
                queue->mempool,
                (num - (queue->block_num - read_index)) * queue->block_size);
    }
    return num;
}

void gen_blk_queue_reset(struct gen_blk_queue *queue)
{
    if (queue)
    {
        queue->read_mirror = 0;
        queue->read_index = 0;
        queue->write_mirror = 0;
        queue->write_index = 0;
    }
}


int gen_blk_queue_block_num(struct gen_blk_queue *queue)
{
    switch (gen_blk_queue_status(queue))
    {
    case GEN_BLK_QUEUE_EMPTY:
        return 0;
    case GEN_BLK_QUEUE_FULL:
        return queue->block_num;
    case GEN_BLK_QUEUE_HALFFULL:
    default:
        if (queue->write_index > queue->read_index)
            return queue->write_index - queue->read_index;
        else
            return queue->block_num - (queue->read_index - queue->write_index);
    };
}




